export var Quaternion = function(x,y,z,w) {
	this.x=x
	this.y=y
	this.z=z
	this.w=w
}

const getLEValue = (b1, b2) => {
	var sign = b2 & (1 << 7)
	var x = (((b2 & 0xFF) << 8) | (b1 & 0xFF))
	var result = x
	if(sign){
		result =  0xFFFF0000 | x
	}
	return result
}

export const getQFromArray = (buffer) => {
	var uint8 = new Uint8Array(buffer)
	var x = getLEValue(uint8[4], uint8[5])/32768
	var y = getLEValue(uint8[6], uint8[7])/32768
	var z = getLEValue(uint8[8], uint8[9])/32768
	var w = getLEValue(uint8[2], uint8[3])/32768
	return new Quaternion(x,y,z,w)
}

export const ab2hex = (buffer) => {
  let hexArr = Array.prototype.map.call(
    new Uint8Array(buffer),
    function(bit) {
      return ('00' + bit.toString(16)).slice(-2)
    }
  )
  return hexArr.join('');
}

export const qMulti = (q0, q1) => {
	var w = q0.w * q1.w - q0.x * q1.x - q0.y * q1.y - q0.z * q1.z
	var x = q0.w * q1.x + q0.x * q1.w + q0.y * q1.z - q0.z * q1.y
	var y = q0.w * q1.y + q0.y * q1.w + q0.z * q1.x - q0.x * q1.z
	var z = q0.w * q1.z + q0.z * q1.w + q0.x * q1.y - q0.y * q1.x
	return new Quaternion(x,y,z,w)
}

export const qConjugate = q0 => {
	return new Quaternion(-q0.x, -q0.y, -q0.z, q0.w)
}

export const getAxisAngles = (q1, q2) => {
	var qd = qMulti(qConjugate(q1),q2)
	var xAngle = Math.atan2( 2 * (qd.w * qd.x + qd.y * qd.z), 1 - 2 * (qd.x**2 + qd.y**2) )
	var xAngleDegree = xAngle * 180 / Math.PI
	// console.log('X-Angle=',xAngleDegree)
	var yAngle = Math.asin ( 2 * (qd.w * qd.y - qd.z * qd.x) )
	var yAngleDegree = yAngle * 180 / Math.PI
	// console.log('Y-Angle=',yAngleDegree)
	var zAngle = Math.atan2( 2 * (qd.w * qd.z + qd.x * qd.y), 1 - 2 * (qd.y**2 + qd.z**2) )
	var zAngleDegree = zAngle * 180 / Math.PI
	// console.log('Z-Angle=',zAngleDegree)
	return [xAngleDegree, yAngleDegree, zAngleDegree]
}